#include <cstdio>
#include <chrono>
#include "rclcpp/rclcpp.hpp"
#include "robot_move_interface/msg/robot_status.hpp"
#include "robot_move_interface/srv/move_robot.hpp"

class Robot{
public:
    Robot()=default;
    ~Robot()=default;

    /**
     * @brief continue to move specified distance
    */
    float move_distance(float distance){
      status_=robot_move_interface::msg::RobotStatus::STATUS_MOVING;
      target_pose_+=distance;
      while(fabs(target_pose_-current_pose_)>0.01){
        float step=distance/fabs(distance)*fabs(target_pose_-current_pose_)*0.1;
        current_pose_+=step;
        std::cout<<"moved: "<<step<<"   "<<"current_pose is: "<<current_pose_<<std::endl;
        std::this_thread::sleep_for(std::chrono::milliseconds(500)); 
      }
      status_=robot_move_interface::msg::RobotStatus::STATUS_STOP;
      return current_pose_;
    }
    /**
     * @brief Get current pose
    */
    int get_current_pose(){
      return current_pose_;
    }
    int get_current_status(){
      return status_;
    }
public:
    float current_pose_=0.0;
    float target_pose_=0.0;
    int status_=robot_move_interface::msg::RobotStatus::STATUS_STOP;
};

class RobotMove:public rclcpp::Node{
public:
    RobotMove(std::string name):Node(name){
      RCLCPP_INFO(this->get_logger(),"I am robot : %s ...",name.c_str());
      timer_=this->create_wall_timer(std::chrono::milliseconds(500),std::bind(&RobotMove::_timer_callback,this));
      move_robot_server_=this->create_service<robot_move_interface::srv::MoveRobot>("move_robot",std::bind(&RobotMove::_handle_move_robot,this,std::placeholders::_1,std::placeholders::_2));
      robot_status_publisher_=this->create_publisher<robot_move_interface::msg::RobotStatus>("robot_topic",10);
    }

private:
    void _timer_callback(){
      robot_move_interface::msg::RobotStatus message;
      message.status=robot_.get_current_status();
      message.pose=robot_.get_current_pose();
      RCLCPP_INFO(this->get_logger(),"Pulishing: %f",message.pose);
      robot_status_publisher_->publish(message);
    }

    void _handle_move_robot(std::shared_ptr<robot_move_interface::srv::MoveRobot_Request> request,std::shared_ptr<robot_move_interface::srv::MoveRobot_Response> response){
      RCLCPP_INFO(this->get_logger(),"Get request info: %f",request->distance);
      robot_.target_pose_+=request->distance;
      robot_.status_=robot_move_interface::msg::RobotStatus::STATUS_MOVING;
      while(fabs(robot_.target_pose_-robot_.current_pose_)>0.01){
        float step=request->distance/fabs(request->distance)*fabs(robot_.target_pose_-robot_.current_pose_)*0.1;
        robot_.current_pose_+=step;
        std::cout<<"moved: "<<step<<"   "<<"current_pose is: "<<robot_.current_pose_<<std::endl;
        _timer_callback();
        std::this_thread::sleep_for(std::chrono::milliseconds(500)); 
      }
      robot_.status_=robot_move_interface::msg::RobotStatus::STATUS_STOP;
      
      response->pose=robot_.get_current_pose();
    }

private:
    Robot robot_{};
    rclcpp::TimerBase::SharedPtr timer_{};
    rclcpp::Service<robot_move_interface::srv::MoveRobot>::SharedPtr move_robot_server_{};
    rclcpp::Publisher<robot_move_interface::msg::RobotStatus>::SharedPtr robot_status_publisher_{};
};

int main(int argc, char ** argv){
  rclcpp::init(argc,argv);
  auto node=std::make_shared<RobotMove>("robot_move_01");
  rclcpp::spin(node);
  rclcpp::shutdown();

  return 0;
}
